from uarm.wrapper import SwiftAPI
import cv2 as cv
import numpy as np


swift = SwiftAPI()
swift.reset()

cap = cv.VideoCapture(4)
pose = []
while True:
    if not cap.isOpened():
        print("相机未打开")
        break
    ret, frame = cap.read()
    if not ret:
        print("无画面")
        break
    cv.imshow("frame", frame)
    key = cv.waitKey(10)
    pos = swift.get_position()
    if key == 27:
        break
    if key == ord("d"):
        swift.set_servo_detach()
    if key == ord("a"):
        swift.set_servo_attach()
    if key == ord("s"):
        print(pos)
        pose.append(pos)
np.savez("./data/base2mark_pose", pose=pose)
cv.destroyAllWindows()
cap.release()

